﻿//2017-12-9 郑捷 创建
#include "ModbusSlaveTCP.h"
/*------------------------------------------------------------------------------------------------
MODBUS-TCP 从站协议

2017-12-27 by 郑捷
1、在MODBUS-RTU的基础上修改为TCP。
---------------------------------------------------------------------------------------------------*/

void ModbusSlaveTCP::begin(EthernetServer *Server,unsigned int *Data,unsigned int Count){
  pModbusServer=Server;
  pDataTable=Data;
  DataTableCount=Count;
  (*pModbusServer).begin();
}

void ModbusSlaveTCP::task(void){
  EthernetClient client=(*pModbusServer).available();  //获取客户端
  if(client){ //有客户端进入
    receiveBytes=client.available();     //接收到的数据
    if(receiveBytes>0){  //有数据收到
      //Serial.print("Client sock:");Serial.print(client.getSocketNumber(),DEC);
      //Serial.print(" receive ");Serial.print(receiveBytes,DEC);Serial.println(" byte.");
      if(receiveBytes<bufferSize){
        client.read(frame,receiveBytes);  //数据存到缓冲区
      }else{
        client.read(frame,bufferSize);
        client.flush();
      }
      dataHandle();  //处理命令
      if(needSendBytes>0){
        //Serial.print("Client sock:");Serial.print(client.getSocketNumber(),DEC);
        //Serial.print(" send ");Serial.print(needSendBytes,DEC);Serial.println(" byte.");
        client.write(frame,needSendBytes);
      }
    }
  }
}

void ModbusSlaveTCP::dataHandle(void){
  unsigned int MBAP_INDEX=frame[0]<<8 | frame[1];  //事物处理标识符
  unsigned int MBAP_VER=frame[2]<<8 | frame[3];   //协议标识符
  unsigned int MBAP_LEN=frame[4]<<8 | frame[5];   //帧长度
  unsigned char MBAP_ID=frame[6];                 //单元标识符号
  unsigned char function=frame[7];                //功能码      
  if(MBAP_LEN+6<=receiveBytes && MBAP_VER==0){   //有效帧长度小于接收到的字节数，且为MODBUS-TCP协议
    if(MBAP_ID==SlaveID || MBAP_ID==0){  //站号匹配或者消息为广播形式
      if (MBAP_ID!=0 && (function == 3)){  //功能码03，不支持广播消息
        unsigned int startData=((frame[8] << 8) | frame[9]);  //读取modbus数据库起始地址           
        unsigned int dataSize=((frame[10] << 8) | frame[11]);  //需要读取的modbus数据库数据长度
        unsigned int endData=startData+dataSize;    //需要读取的modbus数据库数据的结束地址
        unsigned char responseSize=9+dataSize*2;  //计算应答的数据长度
        unsigned int temp;
        if(endData>DataTableCount || responseSize>bufferSize){  //读取数据的结束地址超过了modbus数据库的范围
          responseError(0x02);  //返回错误消息：数据超过范围
        }else{
          //frame[0]-frame[3] MBAP报头与发下来的数据一致
          unsigned int len=3+dataSize*2;  //帧长度
          frame[4]=len>>8;  //帧长度高字节
          frame[5]=len&0xFF;  //帧长度低字节
          frame[6]=SlaveID;  //填写站号,
          //frame[7] 功能码不变
          frame[8]=dataSize*2;  //设定数据长度
          unsigned int index=9; //数据区开始
          for(int i=startData;i<endData;i++){
            temp=pDataTable[i];  //取出modbus数据库中的数据
            frame[index]=temp>>8;
            index++;
            frame[index]=temp&0xFF;
            index++;
          }
          needSendBytes=responseSize;  //返回要发送数据的字节数
        }
      }
      else if(function == 6){  //功能码为06
        unsigned int startData=((frame[8] << 8) | frame[9]);  //写入modbus数据库的地址           
        unsigned int setData=((frame[10] << 8) | frame[11]);  //写入modbus数据库的数值
        if(startData>=DataTableCount){
          responseError(0x02);  //返回错误消息：数据超过范围
        }else{
          pDataTable[startData]=setData;  //写入数据到modbus数据库
          if(MBAP_ID!=0){   //广播方式下不用返回消息
            //功能6的返回消息与指令一致              
            needSendBytes=12;  //返回功能码06的消息              
          }
        }
      }
      else if(function == 16){  //功能码为16时进入条件
        if(frame[12]!=receiveBytes-13){  //校验数据长度
          responseError(0x03);  //返回错误消息：数据长度不符
        }else{  //校验数据长度正确
          unsigned int startData=((frame[8] << 8) | frame[9]);  //写入modbus数据库起始地址           
          unsigned int dataSize=((frame[10] << 8) | frame[11]);  //需要写入的modbus数据库数据长度
          unsigned int endData=startData+dataSize;    //需要写入的modbus数据库数据的结束地址
          if(endData>DataTableCount){  //读取数据的结束地址超过了modbus数据库的范围
            responseError(0x02);  //返回错误消息：数据超过范围
          }else{
            if(frame[12]==(dataSize*2)){        //校验数据长度
              unsigned int index;
              index = 13;  //从数据帧的第13个数据开始读取            
              for(unsigned int i=startData;i<endData;i++){
                pDataTable[i]=(frame[index]<<8|frame[index+1]);  //将数据写入modbus数据库中
                index+=2;
              }
              if(MBAP_ID!=0){   //广播方式下不用返回消息
                //frame[0]-frame[3] MBAP报头与发下来的数据一致
                frame[4] = 0x00;  //帧长度高字节为0
                frame[5] = 6; //帧长度，下面一共6个字节
                frame[6]=SlaveID;  //填写站号
                //frame[7]-frame[11]  不变
                needSendBytes=12;  //发送功能码16的应答数据 
              }
            }else{
              responseError(0x03);  //返回错误消息：数据长度不符
            }
          }    
        }
      }else{  //其他功能码
        responseError(0x01);  //返回错误消息：不支持收到的功能码
      } 
    }
  }
}

//错误信息返回函数
void ModbusSlaveTCP::responseError(unsigned char wrongNumber) 
{
  if(frame[6] == SlaveID){;  //非广播帧
    //frame[0]-frame[3] MBAP报头与发下来的数据一致
    frame[4] = 0x00;
    frame[5] = 3; //下面一共3个字节
    frame[7] = frame[7]+0x80; //功能码首位改1
    frame[8] = wrongNumber;  //填写错误代码
    needSendBytes=9;  //返回错误代码 
  }else{
  	needSendBytes=0;
  }
}
